#include "pid_grando.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
/// <summary>
/// 乘法
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
static float _IQmpy(float a, float b)
{
    return a * b;
}
/// <summary>
/// 设置限制输出
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <param name="c"></param>
/// <returns></returns>
static float _IQsat(float a, float b, float c)
{
    if (a >= b)
        return b;
    if (a <= c)
        return c;
    return a;
}

pid_grando_t *pid_grando_create(float kp, float ki, float kd, int input_max, int input_min, int out_max, int out_min)
{
    // 初始化pid参数
    pid_grando_t *handle = (pid_grando_t *)malloc(sizeof(pid_grando_t));

    handle->param.Kp = kp;
    handle->param.Ki = ki;
    handle->param.Kd = kd;
    // 比例系数权重与微分系数权重
    handle->param.Kr = 1;
    handle->param.Km = 1;
    // 最大输出
    handle->param.Umax = out_max;
    // 最小输入
    handle->param.Umin = out_min;

    // 输入：1
    handle->term.Ref = 0;
    // 目标：2
    handle->term.Fbk = 0;
    handle->term.c1 = 0; // Internal: derivative filter coefficient 1
    handle->term.c2 = 0; // Internal: derivative filter coefficient 2

    handle->data.up = 0;   // Data: proportional term
    handle->data.ui = 0;   // Data: integral term
    handle->data.ud = 0;   // Data: derivative term
    handle->data.v1 = 0;   // Data: pre-saturated controller output
    handle->data.i1 = 0;   // Data: integrator storage: ui(k-1)
    handle->data.d1 = 0;   // Data: differentiator storage: ud(k-1)
    handle->data.d2 = 0;   // Data: differentiator storage: d2(k-1)
    handle->data.w1 = 1.0; // Data: saturation record: [u(k-1) - v(k-1)]
    return handle;
}

void pid_grando_delete(pid_grando_t *handle)
{
    if (NULL == handle)
        return;
    free(handle);
}
/*------------------------------------------------------------------------------
  PID Macro Definition PID执行运算
  set_point:目标值
  input:输入值
  out:输出值
  return ：输出值
------------------------------------------------------------------------------*/
float pid_grando_macro(pid_grando_t *handle, float input, float set_point)
{
    handle->term.Ref = set_point;

    handle->term.Fbk = input;

    /* proportional term */
    handle->data.up = _IQmpy(handle->param.Kr, handle->term.Ref) - handle->term.Fbk;
    printf("input:%f,set-point:%f ,up:%f\n", handle->term.Fbk, handle->term.Ref, handle->data.up);
    /* integral term */
    handle->data.ui = _IQmpy(handle->param.Ki,

                             _IQmpy(handle->data.w1, (handle->term.Ref - handle->term.Fbk))) +
                      handle->data.i1;

    handle->data.i1 = handle->data.ui;

    /* derivative term */
    handle->data.d2 = _IQmpy(handle->param.Kd, _IQmpy(handle->term.c1,
                                                      (_IQmpy(handle->term.Ref, handle->param.Km) - handle->term.Fbk))) -
                      handle->data.d2;
    handle->data.ud = handle->data.d2 + handle->data.d1;
    handle->data.d1 = _IQmpy(handle->data.ud, handle->term.c2);

    /* control output */
    handle->data.v1 = _IQmpy(handle->param.Kp, (handle->data.up + handle->data.ui + handle->data.ud));

    printf("kp:%f,v1:%f ,up:%f,ud:%f\n", handle->param.Kp, handle->data.v1, handle->data.up, handle->data.ud);

    handle->term.Out = _IQsat(handle->data.v1, handle->param.Umax, handle->param.Umin);
    handle->data.w1 = (handle->term.Out == handle->data.v1) ? (1.0) : (0.0);

    return handle->term.Out;
}
void pid_grando_reset(pid_grando_t *handle)
{
     // 比例系数权重与微分系数权重
    handle->param.Kr = 1;
    handle->param.Km = 1;
  

    handle->data.up = 0;   // Data: proportional term
    handle->data.ui = 0;   // Data: integral term
    handle->data.ud = 0;   // Data: derivative term
    handle->data.v1 = 0;   // Data: pre-saturated controller output
    handle->data.i1 = 0;   // Data: integrator storage: ui(k-1)
    handle->data.d1 = 0;   // Data: differentiator storage: ud(k-1)
    handle->data.d2 = 0;   // Data: differentiator storage: d2(k-1)
    handle->data.w1 = 1.0; // Data: saturation record: [u(k-1) - v(k-1)]
}